#include "inc/hw_types.h"		// tBoolean
#include "inc/hw_memmap.h"
#include "utils/uartstdio.h"	// input/output over UART
#include "driverlib/gpio.h"
#include "driverlib/sysctl.h"

#include "RASLib/encoder.h"
#include "RASLib/motor.h"

#include "RobzDemo.h"




void initGo(void) {
	InitializeMotors(true, false);
}

		//924
void Go(signed long ticks) 	//1 tick = 	0.275 cm
{
	signed long encoder0, encoder1;
	signed char left = 0, right = 0, maxSpeed = 127;


	InitializeEncoders(true, false);
	PresetEncoderCounts(0, 0);

	encoder0 = GetEncoderCount(ENCODER_0);	 
	encoder1 = GetEncoderCount(ENCODER_1);
	GetEncoderCounts(&encoder0, &encoder1);

	SetMotorPowers(left, right);

	if(ticks>=0)
	{
		while(encoder0<ticks && encoder1<ticks)
		{
			encoder0 = GetEncoderCount(ENCODER_0);	 
			encoder1 = GetEncoderCount(ENCODER_1);
			GetEncoderCounts(&encoder0, &encoder1);
	
			left = maxSpeed;    
			right = maxSpeed;
			SetMotorPowers(left, right);
		}

//		while(encoder0<ticks)
//			{
//				encoder0 = GetEncoderCount(ENCODER_0);	
//
//				left = maxSpeed;
//				right = 0;
//				SetMotorPowers(left, right);
//			}
//
//		while(encoder1<ticks)
//		{
//			encoder1 = GetEncoderCount(ENCODER_1);
//
//			left = 0;
//			right = maxSpeed;
//			SetMotorPowers(left, right);
//		}
			
	}

	if(ticks<=0)
	{
		while(encoder0>ticks && encoder1>ticks)
		{
			encoder0 = GetEncoderCount(ENCODER_0);	 
			encoder1 = GetEncoderCount(ENCODER_1);
			GetEncoderCounts(&encoder0, &encoder1);
	
			left = -maxSpeed;    
			right = -maxSpeed;
			SetMotorPowers(left, right);
		}


//		while(encoder0>ticks)
//		{
//				encoder0 = GetEncoderCount(ENCODER_0);
//
//				left = -maxSpeed;
//				right = 0;
//				SetMotorPowers(left, right);
//		}
//
//		while(encoder1>ticks)
//		{
//			encoder1 = GetEncoderCount(ENCODER_1);
//
//			left = 0;
//			right = -maxSpeed;
//			SetMotorPowers(left, right);
//	   	}
	}
	
	
	SetMotorPowers(0,0);

}

void initTurn(void) {
	InitializeMotors(true, false);
}


 // 90 deg = 119  
 // -90 deg = -114
 
 // 180 deg = 238  
 // -180 deg = -228 

  void Turn(signed long angle) //diameter 21.7cm, circumference 68.1726cm
{
	 signed long turnticks;
	 signed long encoder0, encoder1;
	 signed char left = 0, right = 0, maxSpeed = 100;


	InitializeEncoders(true, false);
	PresetEncoderCounts(0, 0);

	GetEncoderCounts(&encoder0, &encoder1);
	turnticks = angle;
	UARTprintf("%d", turnticks);
	UARTprintf("%d", angle);

	 // Clockwise  OR turn right.

	 if(angle>0)//for angles 0-180 turn right
	 {

		while(encoder1 < turnticks)
		{	 
			encoder1 = GetEncoderCount(ENCODER_1);
	
			left = 0;
			right = maxSpeed;
			SetMotorPowers(left, right);
		}
	   }


	if(angle<0)//for angles 0-180 turn left
	 {
	 	 	UARTprintf("\hihhihi\n");
		while(encoder0 < -turnticks)
		{
		  	encoder0 = GetEncoderCount(ENCODER_0);	 
			left = maxSpeed;
			right = 0;
			SetMotorPowers(left, right);
		}
	  }

	
	 SetMotorPowers(0, 0);	
}
 




// BELOW IS ADAMS FAST TURN

// Just time in the angle measure you would like to turn....
 
void FastTurn(double angle) //diameter 21.7cm, circumference 68.1726cm
{
	 signed long turnticks;
	 signed long encoder0, encoder1;
	 signed char left = 0, right = 0, maxSpeed = 127;


	InitializeEncoders(true, false);
	PresetEncoderCounts(0, 0);

	encoder0 = GetEncoderCount(ENCODER_0);	 
	encoder1 = GetEncoderCount(ENCODER_1);
	GetEncoderCounts(&encoder0, &encoder1);


	if(angle<=180)
		turnticks = ((angle/360)*248);	//fraction of a complete rotation times 
	 									//the # of turns in a complete rotation

	if(angle>180)
		turnticks = (((360-angle)/360)*248);
	
	UARTprintf("%d", turnticks);

	 if(angle<=180)//for angles 0-180 turn right
	 {
	 	while(encoder0 > -turnticks && encoder1 < turnticks)
		{
		  	encoder0 = GetEncoderCount(ENCODER_0);	 
			encoder1 = GetEncoderCount(ENCODER_1);
			GetEncoderCounts(&encoder0, &encoder1);
	
			left = -maxSpeed;
			right = maxSpeed;
			SetMotorPowers(left, right);
		}
	
	
	
		while(encoder0 > -turnticks)
		{
		  	encoder0 = GetEncoderCount(ENCODER_0);	 

			left = -maxSpeed;
			right = 0;
			SetMotorPowers(left, right);
		}


		while(encoder1 < turnticks)
		{	 
			encoder1 = GetEncoderCount(ENCODER_1);
	
			left = 0;
			right = maxSpeed;
			SetMotorPowers(left, right);
		}	
	   }


	if(angle>180)//for angles 0-180 turn left
	 {
	 	while(encoder0 < turnticks && encoder1 > -turnticks)
		{
		  	encoder0 = GetEncoderCount(ENCODER_0);	 
			encoder1 = GetEncoderCount(ENCODER_1);
			GetEncoderCounts(&encoder0, &encoder1);
	
			left = maxSpeed;
			right = -maxSpeed;
			SetMotorPowers(left, right);
		}

		while(encoder0 < turnticks)
		{
		  	encoder0 = GetEncoderCount(ENCODER_0);	 

			left = maxSpeed;
			right = 0;
			SetMotorPowers(left, right);
		}


		while(encoder1 > -turnticks)
		{	 
			encoder1 = GetEncoderCount(ENCODER_1);
	
			left = 0;
			right = -maxSpeed;
			SetMotorPowers(left, right);
		}
		
	   }



	UARTprintf("%d", turnticks);
	 SetMotorPowers(0, 0);	
}



